#ifndef A_STAR_PLANNER_H
#define A_STAR_PLANNER_H

#include <ros/ros.h>
#include <costmap_2d/costmap_2d.h>
#include <costmap_2d/costmap_2d_ros.h>
#include <nav_core/base_global_planner.h>
#include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/Path.h>
#include <tf2_ros/buffer.h>
#include <tf2/convert.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

#include <vector>
#include <queue>
#include <unordered_set>
#include <cmath>

namespace my_astar_planner {

class AStarPlanner : public nav_core::BaseGlobalPlanner {
public:
    AStarPlanner();
    AStarPlanner(std::string name, costmap_2d::Costmap2DROS* costmap_ros);
    
    void initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros) override;
    bool makePlan(const geometry_msgs::PoseStamped& start,
                 const geometry_msgs::PoseStamped& goal,
                 std::vector<geometry_msgs::PoseStamped>& plan) override;

private:
    struct Node {
        int x, y;
        double g, h, f;
        Node* parent;
        
        Node(int x, int y, double g, double h, Node* parent = nullptr)
            : x(x), y(y), g(g), h(h), f(g + h), parent(parent) {}
        
        bool operator<(const Node& other) const {
            return f > other.f; // For priority queue (min-heap)
        }
    };

    struct NodeHash {
        size_t operator()(const Node* node) const {
            return node->x * 10000 + node->y; // Simple hash for grid coordinates
        }
    };

    struct NodeEqual {
        bool operator()(const Node* a, const Node* b) const {
            return a->x == b->x && a->y == b->y;
        }
    };

    costmap_2d::Costmap2DROS* costmap_ros_;
    costmap_2d::Costmap2D* costmap_;
    bool initialized_;
    double resolution_;
    int width_, height_;
    ros::Publisher plan_pub_;

    std::vector<geometry_msgs::PoseStamped> getPath(Node* node);
    double heuristic(int x1, int y1, int x2, int y2);
    bool isValid(int x, int y);
    bool isFree(int x, int y);
    void worldToMap(double wx, double wy, int& mx, int& my);
    void mapToWorld(int mx, int my, double& wx, double& wy);
    void publishPlan(const std::vector<geometry_msgs::PoseStamped>& path);
};

} // namespace a_star_planner

#endif // A_STAR_PLANNER_H